#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>  /* UNIX standard function definitions */
#include <fcntl.h>   /* File control definitions */
#include <errno.h>   /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */

#include <config.h>
#include <systems.h>
#include <misc.h>
#include <drivers/bluetooth/btcore.h>
#include <drivers/bluetooth/hsm30.h>

#define BLUETOOTH_DEVICE "/dev/ttyS1"
#define READ_BUF_LEN 256

int bt_dev; /* File descriptor for the port */
u8 data_buf[READ_BUF_LEN];
int data_len = 0;
BT_PACKET *pkt_buffer[16];

struct bluetooth_operations bt_ops = {
    .init = hsm30_init,
    .write = hsm30_write,
    .read = hsm30_read,
};

struct bluetooth_operations *bluetooth_ops = &bt_ops;

int hsm30_init(void)
{
    struct termios options;

    bt_dev = open(BLUETOOTH_DEVICE, O_RDWR | O_NOCTTY | O_NONBLOCK);
    if(bt_dev == -1)
    {
        ERROR("Unable to open %s\n", BLUETOOTH_DEVICE);
        return -1;
    }
    
    /* Get the current options for the port */
    tcgetattr(bt_dev, &options);
    
    /* Set Baudrate to 19200 */
    cfsetispeed(&options, B19200);
    cfsetospeed(&options, B19200);
    
    options.c_cflag |= CREAD;
    
    /* No parity (8N1) */
    options.c_cflag &= ~PARENB;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;
    
    /* Using raw input */
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_iflag = IGNPAR;
    
    /* Using raw output */
    options.c_oflag = 0;

    /* Set the new options for the port */
    tcflush(bt_dev, TCIFLUSH);
    tcsetattr(bt_dev, TCSANOW, &options);
    
    memset(data_buf, 0x0, READ_BUF_LEN);
    return 0;
}

int hsm30_write(BT_PACKET *packet)
{
    char *buf;
    int len = 0;
    BOOLEAN no_crlf = false;
    
    if(!packet)
        return -1;
    
    len = sizeof(packet->command) + packet->data_len + 2;
    
    if(packet->command == HOST_BYPASS_IPOD_CMD) {
        no_crlf = true;
        len -= 2;
    }
        
    buf = malloc(len);
    
    buf[0] = packet->command;
    if(packet->data_len > 0)
        memcpy(&buf[1], packet->data, packet->data_len);
    if(!no_crlf) {
        buf[packet->data_len + 1] = 0xd;
        buf[packet->data_len + 2] = 0xa;
    }
    
    len = write(bt_dev, buf, len);
    free(buf);
    
    if(len < 0) {
        ERROR("Unable to write data to %s\n", BLUETOOTH_DEVICE);
        return len;
    }
    
    return 0;
}


BT_PACKET *hsm30_read(void)
{
    BT_PACKET *packet;
    int nbytes, i;
    BOOLEAN first_loop = true, no_crlf = false;
    char *p = NULL, *eop = "\r\n";
    
    while((nbytes = read(bt_dev, &data_buf[data_len], READ_BUF_LEN - data_len)) > 0)
    {
        if(first_loop && data_buf[0] == HSM_IPOD_RESPONSE_DATA)
            no_crlf = true;
        
        first_loop = false;
        data_len += nbytes;
        //DPRINTF("\nData length: %d, nbytes: %d\nData: ", data_len, nbytes);
        //for(i = 0; i < nbytes; i++)
        //    DPRINTF("%x ", data_buf[data_len - nbytes + i]);
            
        if(!no_crlf) {
            if((p = _strnstr(data_buf, eop, data_len)) == NULL) {
                if(data_len == READ_BUF_LEN) {
                    DPRINTF2("Data buffer exceeded\n");
                    return NULL;
                }
                else if(data_len > READ_BUF_LEN) {
                    DPRINTF2("*Fatal Error* Data buffer overflow\n");
                    return NULL;
                }
                continue;
            }
            //DPRINTF("\nEOP found\n");
            /* The end bytes of the packet is found */
            packet = (BT_PACKET *)malloc(sizeof(BT_PACKET));
            packet->command = data_buf[0];
            packet->data_len = (int)p - (int)&data_buf[1];
            //DPRINTF("Packet data length: %d\n", packet->data_len);
            if(packet->data_len > 0) {
                packet->data = malloc(packet->data_len);
                memcpy(packet->data, &data_buf[1], packet->data_len);
            }
            else {
                packet->data = NULL;
                if(packet->data_len < 0) {
                    ERROR("%d-byte of data is missing\n", 0-packet->data_len);
                    free(packet);
                    data_len = 0;
                    return NULL;
                }
            }
                
            data_len -= (packet->data_len + 3);
            //DPRINTF("Remaining data length of buffer: %d\n", data_len);
            if(data_len > 0) {
                memcpy(&data_buf[0], (char *)((int)p + 2), data_len);
            }
            
            DPRINTF("\nRaw packet (len=%d): ", packet->data_len);
            for(i = 0; i < packet->data_len + 1; i++) {
                if(i == 5) break;
                DPRINTF("%c ", data_buf[i]);
            }
            DPRINTF("\n");
            //DPRINTF("0x%x 0x%x\n", data_buf[i], data_buf[i+1]);
            memset(&data_buf[data_len], 0x0, READ_BUF_LEN - data_len);
            
            return packet;
        }
        else {
            /* no_crlf == true -> this packet is "iPod Response Data" */
            DPRINTF("Packet type is not implement yet!\n");
            return NULL;
        }
    }
    
    /* No data */
    return NULL;
}
